//
//  Line.c
//  Vector_Math
//
//  Created by John R Peterson on 3/11/12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//

#include <stdio.h>
#include "Line.h"

// internal functions that noone else should use
void Line_accumulate_INTERNAL(Line *this, Vec *v);
void Line_wrapUp_INTERNAL(Line *this);
void Line_unnormalize_INTERNAL(Line* this);
// internal functions
void PolyLine_updatePos_INTERNAL(PolyLine *this, Vec *pos);

// only works for 2D vectors but would be expensive to check for this
// also be aware that Line will edit the vec_list given to it
Line* Line_init(Vec_list *P) {
    if (P->size < 2) {
        printf("Warning: need atleast 2 points to construct a line: Line_init\n");
        exit(1);
    }
    
    // allocate space for the line itself
    Line* this = (Line *) malloc(sizeof(Line));
    if (this == NULL) {
        printf("Warning: malloc failed: Line_init:1 \n");
        exit(1);
    }
    
    // initialize zero matricies and vectors
    // TODO may not need to do all of these here!
    this->centroid = Vec_createZeros(2);
    this->Sigma = Matrix_createZeros(2,2);
    this->points = Vec_list_createS((P->size) * 2); // initialize empty list
    
    // for each point
    int i;
    for (i = 0; i < P->size; i++) {
        Line_accumulate_INTERNAL(this, P->list[i]);
    }
    // wrap up
    Line_wrapUp_INTERNAL(this);
    
    return this;
}

// only works for 2D vectors but would be expensive to check for this
// also be aware that Line will edit the vec_list given to it
Line* Line_2Point(Vec *a, Vec *b) {
    
    // allocate space for the line itself
    Line* this = (Line *) malloc(sizeof(Line));
    if (this == NULL) {
        printf("Warning: malloc failed: Line_init:1 \n");
        exit(1);
    }
    
    // initialize zero matricies and vectors
    // TODO may not need to do all of these here!
    this->centroid = Vec_createZeros(2);
    this->Sigma = Matrix_createZeros(2,2);
    this->points = Vec_list_createS(4); // initialize empty list
    
    // for each point
    Line_accumulate_INTERNAL(this, a);
    Line_accumulate_INTERNAL(this, b);
   
    // wrap up
    Line_wrapUp_INTERNAL(this);
    
    return this;
}

// add a single point to the line
void Line_add(Line *this, Vec *v) {
    Line_unnormalize_INTERNAL(this);
    Line_accumulate_INTERNAL(this, v);
    Line_wrapUp_INTERNAL(this);
}

void Line_addMany(Line *this, Vec_list *vlist) {
    Line_unnormalize_INTERNAL(this);
    int i;
    for (i = 0; i < vlist->size; i++) {
        Line_accumulate_INTERNAL(this, vlist->list[i]);
    }
    Line_wrapUp_INTERNAL(this);
}

// destroys a line
void Line_destroy(Line *this) {
    
    // destroy all the matricies
    Matrix_destroy(this->Sigma);
    Vec_destroy(this->centroid);
    Vec_destroy(this->direction);
    Vec_destroy(this->Lambda);
    
    // destroy the Vec_list
    Vec_list_destroy(this->points);
    
    // destroy the structure itself
    free(this);
}

// destroys a line and all the points that it points to
// use with caution
void Line_destroyR(Line *this) {
    
    // destroy all the matricies
    Matrix_destroy(this->Sigma);
    Vec_destroy(this->centroid);
    Vec_destroy(this->direction);
    Vec_destroy(this->Lambda);
    
    // destroy the Vec_list
    Vec_list_destroyR(this->points);
    
    // destroy the structure itself
    free(this);
}

// computes perpendicular distance from line to the point
float Line_distance(Line *this, Vec *p) {
    if ((p->size != 2) && (p->size != 3)) {
        printf("Warning only supports 2D or 2D homogeneous vectors: Line_distance\n");
        exit(1);
    }
    float d = Vec_L2norm(Vec_subtract(Vec_subtract(this->centroid, p), Vec_scale(this->direction, Vec_dotProduct(Vec_subtract(this->centroid, p), this->direction))));
    printf("perpendicular distance = %f\n",d);
    return d;
}

// computes parallel distace along line from centroid to point
float Line_distanceP(Line *this, Vec *p) {
    if ((p->size != 2) && (p->size != 3)) {
        printf("Warning only supports 2D or 2D homogeneous vectors: Line_distanceP\n");
        exit(1);
    }
    return fabsf(Vec_dotProduct(Vec_subtract(this->centroid,p), this->direction));
}

// interperets major axis as a line between two points and returns its length
// not strictly correct for lines that contain more than two points
float Line_length(Line *this) {
    return 2*sqrt(this->Lambda->val[0]);
}

// for internal use only!
void Line_accumulate_INTERNAL(Line* this, Vec *v) {
    // accumulate for centroid comutation
    if (!(v->isColumn)) {
        v = Vec_transpose(v);
    }
    Vec_addI(this->centroid, v);
    Matrix_addI(this->Sigma, Vec_multiply(v,Vec_transpose(v)));
    Vec_list_add(this->points,v);
}

// for internal use only!!!
void Line_wrapUp_INTERNAL(Line *this) {
    float sf = 1.0/(this->points->size);
    
    // compute expected values
    Vec_scaleI(this->centroid,sf);
    Matrix_scaleI(this->Sigma,sf);
    
    // compute sigma
    Matrix_subtractI(this->Sigma, Vec_multiply(this->centroid,Vec_transpose(this->centroid)));
    
    // use dominant eigne vector of sigma
    Matrix* ans[2];
    Matrix_eigDecomp(this->Sigma, ans);
    // record our eigenvalues in a vector
    float eigV[] = {ans[0]->val[0][0],ans[0]->val[1][1]};
    this->Lambda = Vec_create(eigV, 2);
    Matrix_destroy(ans[0]);
    // record the dominant eigen vector
    this->direction = Matrix_getColumn(ans[1],0); // 1st column contains dominant eigenvector
    Matrix_destroy(ans[1]);
    
}

// for internal use only!!
void Line_unnormalize_INTERNAL(Line *this) {
    float sf = (float) this->points->size;
    Vec_scaleI(this->centroid,sf);
    Matrix_scaleI(this->Sigma,sf);
}

/***************************************************************/

PolyLine* PolyLine_init(Vec_list *verticies) {
    
    // allocate room for the structure
    PolyLine *pL = (PolyLine *) malloc(sizeof(PolyLine));
    if (pL == NULL) {
        printf("Warning: malloc failed: PolyLine_init:1\n");
        exit(1);
    }
    pL->vertices = Vec_list_copy(verticies); // make a copy of it so we can destroy without worry
    // initialize space for the list of line pointers
    pL->maxsize = (verticies->size - 1) * 2;
    pL->Llist = (Line **) malloc(sizeof(Line *) * pL->maxsize);
    if (pL->Llist == NULL) {
        printf("Warning: malloc failed: PolyLine_init:2\n");
        exit(1);
    }
    
    // create lines and add to the list
    int i;
    for (i = 1; i < verticies->size; i++) {
        pL->Llist[i-1] = Line_2Point(verticies->list[i-1],verticies->list[i]);
    }
    pL->size = verticies->size - 1;
    
    // assume that we start at the begining of the polyline
    pL->pos = 0;
    
    return pL;
}

// appends an extra vertex to the end of the poly line and 
void PolyLine_addVertexEnd(PolyLine *this, Vec *v) {
   
    // if the line list is full need to allocate more space
    if (this->size >= this->maxsize) {
        this->Llist = (Line **) realloc(this->Llist, sizeof(Line *) * this->maxsize * 2);
        if (this->Llist == NULL) {
            printf("Warning: realloc failed PolyLine_addVertexEnd\n");
            exit(1);
        }
        this->maxsize = this->maxsize * 2;
    }
    
    // now create a new line
    this->Llist[this->size] = Line_2Point(this->vertices->list[this->vertices->size - 1], v);
    (this->size)++;
    // add point to list
    Vec_list_add(this->vertices,v);
}

// doesn't destroy the underlying verticies
void PolyLine_destroy(PolyLine *this) {
    
    // first destroy every line
    int i;
    for (i = 0; i < this->size; i++) {
        Line_destroy(this->Llist[i]);
    }
    
    // then destroy the line list
    free(this->Llist);
    
    // now destroy the vertex list
    Vec_list_destroy(this->vertices);
    
    // now destroy the whole structure
    free(this);
}


// computes perpendicular distance from the position vector to the polyline
// the lines on either end are considered to be infinitely long, so will return erros to those
// when it leaves the line
float PolyLine_error(PolyLine *this, Vec *pos) {
    // update the polyline to reflect the closest segment
    PolyLine_updatePos_INTERNAL(this, pos);
    
    // use the closest segment to compute an error
    return Line_distance(this->Llist[this->pos], pos);
}

// removes a vertex from the beginning of the polyline and destroys the line
// does not destroy the underlying vertex, just removes it from the list
void PolyLine_removeVertexStart(PolyLine *this) {
    
    // destroy the line
    Line_destroy(this->Llist[0]);
    
    // shift the pointers
    memcpy(&(this->Llist[0]),&(this->Llist[1]),sizeof(Line *) * ((this->size) - 1));
    
    // remove the vertex
    Vec_list_remove(this->vertices,0);
    
    // decrement size
    (this->size)--;
    // decrement position to compensate unless already at the beginning
    if (this->pos > 0) {
        (this->pos)--;
    } else {
        this->pos = 0; // set it just to be safe
    }
}

// removes a vertex from the beginning of the polyline and destroys the line
// destroys the vertex as well as removes it from the list
void PolyLine_removeVertexRStart(PolyLine *this) {
    
    // destroy the line
    Line_destroy(this->Llist[0]);
    
    // shift the pointers
    memcpy(&(this->Llist[0]),&(this->Llist[1]),sizeof(Line *) * ((this->size) - 1));
    
    // remove the vertex
    Vec_list_removeR(this->vertices,0);
    
    // decrement size
    (this->size)--;
    // decrement position to compensate unless already at the beginning
    if (this->pos > 0) {
        (this->pos)--;
    } else {
        this->pos = 0; // set it just to be safe
    }
}

// given the current position, and the poly line, figure out which line we should measure our error to
// using nearest neighbor association to their centroids, only examines a subset of all lines in the polyline
// TODO a sensible transfer between line segements!!!
void PolyLine_updatePos_INTERNAL(PolyLine *this, Vec *pos) {
    
    float dprev,dcur,dnext;
    int cur = this->pos;
    // compute distance to the centroid of the current, previous, and next line segments, 
    // and pick the closest one
    if ((this->pos > 0) && this->pos < (this->size -1)) {
        // previous distance
        dprev = Vec_L2norm2(Vec_subtract(this->Llist[cur - 1]->centroid,pos));
        // current distance
        dcur = Vec_L2norm2(Vec_subtract(this->Llist[cur]->centroid,pos));
        // next distance
        dnext = Vec_L2norm2(Vec_subtract(this->Llist[cur+1]->centroid,pos));
        
        if ((dprev < dcur) && (dprev < dnext)) {
            this->pos = cur-1;
        } else if ((dnext < dprev) && (dnext < dcur)) {
            this->pos = cur+1;
        }
        // and don't update otherwise since current is just fine
    } else if (this->pos == 0) {
        // current distance
        dcur = Vec_L2norm2(Vec_subtract(this->Llist[cur]->centroid,pos));
        // next distance
        dnext = Vec_L2norm2(Vec_subtract(this->Llist[cur+1]->centroid,pos));
        
        if (dnext < dcur) {
            this->pos = cur+1;
        }

    } else if (this->pos == (this->size -1)) {
        // previous distance
        dprev = Vec_L2norm2(Vec_subtract(this->Llist[cur - 1]->centroid,pos));
        // current distance
        dcur = Vec_L2norm2(Vec_subtract(this->Llist[cur]->centroid,pos));
        
        if (dprev < dcur) {
            this->pos = cur-1;
        }
    } else {
        printf("Warning invalid position\n");
    }
}


